#include "pcbqueue.h"
#include "globals.h"
#include "debug.h"

VOID enqueue_pcb(PCB_QUEUE* queue, PCB* pcb) {
	//debug_out_string("enqueue_pcb START\n\r");
	if(queue->tail != NULL) {
		//debug_out_string("tail !NULL\n\r");
		queue->tail->next = pcb;
	} else {
		//debug_out_string("tail NULL\n\r");
		queue->head = pcb;
	}
	queue->tail = pcb;
	//debug_out_string("enqueue_pcb END\n\r");
}

PCB* dequeue_pcb(PCB_QUEUE* queue) {
	if(queue->head != NULL) {
		PCB* pcb = queue->head;
		queue->head = pcb->next;
		if(queue->head == NULL) {
			queue->tail = NULL;
		}
		return pcb;
	}
	return NULL;
}

VOID priority_enqueue(PCB_PRIORITY_QUEUE* priority_queue, PCB* process) {
	//debug_out_string("priority_enqueue START\n\r");
	enqueue_pcb(&(priority_queue->queues[process->priority]), process);
	//debug_out_string("priority_enqueue END\n\r");
}

PCB* priority_dequeue(PCB_PRIORITY_QUEUE* priority_queue) {
	PCB* pcb;

	UINT16 i = 0;
	while( i < NUM_PRIORITIES ) {
		pcb = dequeue_pcb(&priority_queue->queues[i]);
		if(pcb != NULL) {
			return pcb;
		}
		i++;
	}

	return NULL;
}

SINT32 priority_remove(PCB_PRIORITY_QUEUE* priority_queue, PCB* process) {
	PCB* test;
	PCB* prev;

	UINT16 i = 0;
	while( i < NUM_PRIORITIES ) {
		PCB_QUEUE* queue = &priority_queue->queues[i];
		prev = NULL;
		test = queue->head;
		while( test != process && test != NULL ) {
			prev = test;
			test = test->next;
		}
		if( test != NULL ) {
			if( prev != NULL ) {
				prev->next = process->next;
			} else {
				queue->head = process->next;
			}

			process->next = NULL;

			return RTX_SUCCESS;
		}

		i++;
	}

	return RTX_ERROR;
}

VOID rpq_enqueue(PCB* process) {
	//debug_out_string("RPQ_ENQUEUE START\n\r");
	priority_enqueue(&ready_queue, process);
	//debug_out_string("RPQ_ENQUEUE END\n\r");
}

PCB* rpq_dequeue() {
	PCB* pcb = priority_dequeue(&ready_queue);

	if( pcb != NULL ){
		return pcb;
	}

	return get_pcb(NULL_PROCESS); //Change to Null Process
}

SINT32 rpq_remove(PCB* process) {
	return priority_remove(&ready_queue, process);
}
